#include "MPC5604B_M07N.h"
#include "delays.h"
#include "utils.h"
#include "driver.h"
#include <math.h>

vuint32_t j;                					/* Dummy idle counter */
volatile uint8_t Result[128], Result2[128], a1,a2,a3;                	/* Read converstion result from ADC input ANS0 */
volatile uint32_t chw,adcdata,adcdata2,curdata,max1,min1,max2,min2,th1,th2;
#define treshold 90
#define treshold2 100
#define TRUE 1
#define FALSE 0
#define HIGH 1
#define LOW 0
#define pi 3.141592
#define atraso 3
#define coef01 0.60
#define coef02 0.60

uint8_t i,d1,faixa,faixa2,centro,centro2,d2,h,contador;
double coef1,coef2,ang;
int em_faixa,em_faixa2;
vuint32_t total, fila[atraso];

void CAMERA(void)
{

	SIU.PCR[7].R = 0x0200;				/* Program the Sensor read start pin as output*/
	SIU.PCR[78].R = 0x0200;
	SIU.PCR[9].R = 0x0200;
	SIU.PCR[79].R = 0x0200;
	SIU.PCR[15].R = 0x0200;				/* Program the Sensor Clock pin as output*/
	SIU.PCR[14].R = 0x200;
	SIU.PCR[12].R = 0x200;
/*	contador=1;
	for(i=1;i<(atraso+1);i++)
	{
		fila[i]=1500;
	}
*/
	for(;;)
		{	
				
		for(j=0;j<2;j++)
		{	
		
			SIU.GPDO[14].B.PDO=HIGH;
			SIU.GPDO[12].B.PDO=HIGH;
			
			
			SIU.GPDO[7].B.PDO = 0; 	//CK
			SIU.GPDO[9].B.PDO = 0;	//SI
			SIU.GPDO[9].B.PDO = 1;	//SI	
			SIU.GPDO[78].B.PDO = 0;	//CK
			SIU.GPDO[79].B.PDO = 0;	//SI
			SIU.GPDO[79].B.PDO = 1;	//SI
			SIU.GPDO[15].B.PDO = 1;	//PULSO
			DelayMEU(30);
			SIU.GPDO[7].B.PDO = 1;	//CK
			SIU.GPDO[78].B.PDO = 1;	//CK
			DelayMEU(30);
			SIU.GPDO[9].B.PDO = 0;	//SI
			SIU.GPDO[79].B.PDO = 0;	//SI
			SIU.GPDO[15].B.PDO = 0;	//PULSO
			DelayMEU(30);
			SIU.GPDO[7].B.PDO = 0;	//CK
			SIU.GPDO[78].B.PDO = 0;	//CK
			
		
			SIU.GPDO[14].B.PDO=LOW;
			SIU.GPDO[12].B.PDO=LOW;
		}
		
			
		DelayMEU(30);
		//em_faixa=FALSE;
		for (i=1;i<128;i++)
		{
			DelayMEU(30);
			SIU.GPDO[7].B.PDO = 1;	/* Sensor Clock High */
			SIU.GPDO[78].B.PDO = 1;
			ADC.MCR.B.NSTART=1;     		/* Trigger normal conversions for ADC0 */
			while (ADC.MCR.B.NSTART == 1) {};
			adcdata = ADC.CDR[2].B.CDATA;
			adcdata2 = ADC.CDR[0].B.CDATA;
			DelayMEU(30);
			SIU.GPDO[7].B.PDO = 0;	/* Sensor Clock Low */
			SIU.GPDO[78].B.PDO = 0;
			Result[i] = (uint8_t)(adcdata >> 2);
			Result2[i] = (uint8_t)(adcdata2 >> 2);
			
			
		/*	
			if(Result[i]<=treshold)
			{
				SIU.GPDO[14].B.PDO=HIGH;
				if(em_faixa==FALSE)
				{
					d1=i;
					em_faixa=TRUE;
				}
			}
			if(Result[i]>=treshold)
			{
				SIU.GPDO[14].B.PDO=LOW;
				if(em_faixa==TRUE)
				{
					faixa=i-d1;
					centro=(faixa/2)+d1;
					em_faixa=FALSE;
				}
			}
			
				
		*/
		}
		em_faixa=FALSE;
		em_faixa2=FALSE;
		max1=0;
		max2=0;
		min1=255;
		min2=255;
		for (i=10;i<118;i++)
		{
			if(Result[i]>max1)
			{
				max1=Result[i];
			}
			
			if(Result2[i]>max2)
			{
				max2=Result2[i];
			}
			
			/*
			if(Result[i]<min1)
			{
				min1=Result[i];
			}
			
			if(Result2[i]<min2)
			{
				min2=Result2[i];
			}
			*/
		}
		th1=(max1)*coef01;
		th2=(max2)*coef02;
		
		for (i=10;i<118;i++)
		{
			//tratamento 1
			if(Result[i]<=th1)
			{
				SIU.GPDO[14].B.PDO=HIGH;
				if(em_faixa==FALSE)
				{
					d1=i;
					em_faixa=TRUE;
				}
			}
			if(Result[i]>=th1)
			{
				SIU.GPDO[14].B.PDO=LOW;
				if(em_faixa==TRUE)
				{
					faixa=i-d1;
					centro=(faixa/2)+d1;
					em_faixa=FALSE;
				}
			}
			//tratamento 2
			if(Result2[i]<=th2)
			{
				SIU.GPDO[12].B.PDO=HIGH;
				if(em_faixa2==FALSE)
				{
					d2=i;
					em_faixa2=TRUE;
				}
			}
			if(Result2[i]>=th2)
			{
				SIU.GPDO[12].B.PDO=LOW;
				if(em_faixa2==TRUE)
				{
					faixa2=i-d2;
					centro2=(faixa2/2)+d2;
					em_faixa2=FALSE;
				}
			}

			
		}
	//	em_faixa=FALSE;
	/*	for (i=5;i<124;i++)
		{
		
			if(Result2[i]<=treshold)
			{
				SIU.GPDO[12].B.PDO=HIGH;
				if(em_faixa==FALSE)
				{
					d2=i;
					em_faixa=TRUE;
				}
			}
			if(Result2[i]>=treshold)
			{
				SIU.GPDO[12].B.PDO=LOW;
				if(em_faixa==TRUE)
				{
					faixa=i-d2;
					centro2=(faixa/2)+d2;
					em_faixa=FALSE;
				}
			}
			
			
		}
		*/
		
	//	EMIOS_0.CH[4].CBDR.R = 1500;
		SIU.GPDO[14].B.PDO = LOW;
		SIU.GPDO[12].B.PDO=LOW;
		
		//total=(centro*5.46)+1150;
		//EMIOS_0.CH[4].CBDR.R = total;
		
		h=10;
		coef1=(0.1719*centro)+5;
		coef2=(0.25*centro2);
		ang=(atan((coef2-coef1)/h))*(180/pi);
		
		total=1500-(6.0345*ang);
		
		if (total<1150)
		{
			total=1150;
		}
		
		if (total>1850)
		{
			total=1850;
		}
				
		EMIOS_0.CH[4].CBDR.R = total;
		
		//total=1150+(centro*5.46);
/*
		fila[contador]=total;
		if(contador==atraso)
		{
			EMIOS_0.CH[4].CBDR.R = fila[1];
		}
		else
		{
			EMIOS_0.CH[4].CBDR.R = fila[(contador+1)];
		}
		
		
		contador++;
		if (contador==(atraso+1))
		{
			contador=1;
		}
		
		*/
		
		Delaycamera();
		/*
		for(j=0;j<2;j++)
		{	
			SIU.GPDO[78].B.PDO = 0;
			SIU.GPDO[79].B.PDO = 0;		
			SIU.GPDO[79].B.PDO = 1;		
			DelayMEU(30);
			SIU.GPDO[78].B.PDO = 1;	
			DelayMEU(30);
			SIU.GPDO[79].B.PDO = 0;
			DelayMEU(30);
			SIU.GPDO[78].B.PDO = 0;
		}
		DelayMEU(30);
		//em_faixa=FALSE;
		for (i=1;i<128;i++)
		{
			DelayMEU(30);
			SIU.GPDO[78].B.PDO = 1;
			DelayMEU(30);
			SIU.GPDO[78].B.PDO = 0;

		}
		*/
		
		Delaycamera2();
	}
		
}